/*
 * OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
 *
 * Copyright (C) 2022 i2Nav Group, Wuhan University
 *
 *     Author : Hailiang Tang
 *    Contact : thl@whu.edu.cn
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef INTEGRATION_DEFINE_H
#define INTEGRATION_DEFINE_H

#include <Eigen/Geometry>
#include <vector>

using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using std::vector;

/*
double time;: 时间戳，表示当前状态的时间。
Vector3d p;: 位置向量，通常表示在某个坐标系下的三维位置坐标（例如，ENU或NED坐标系）。
Quaterniond q;: 四元数，用于表示当前姿态（方向或旋转），相比欧拉角，它避免了万向节锁的问题。
Vector3d v;: 速度向量，表示物体在三维空间中的速度。
Vector3d bg;: 陀螺仪零偏（偏差），用于表示陀螺仪的误差。
Vector3d ba;: 加速度计零偏（偏差），用于表示加速度计的误差。
Vector3d s;: 某个标量参数的三维表示，具体用途需根据上下文确定。
double sodo;: 里程计速度，用于描述车辆的线速度。
Vector2d abv;: 安装角差异，用于表示两个坐标系之间的角度差异。
Vector3d sg;: 陀螺仪比例因子误差。
Vector3d sa;: 加速度计比例因子误差。

2. IntegrationStateData 结构体
这个结构体用于存储导航状态数据，包括姿态、速度、偏差和一些混合参数。
速度和偏差（9个元素）
速度、偏差和里程计（10个元素）
速度、偏差、里程计和abv（12个元素）
速度、偏差和比例因子（15个元素）
速度、偏差、里程计和比例因子（16个元素）
速度、偏差、里程计、比例因子和abv（18个元素）
double time;: 时间戳，表示当前状态的时间。
double pose[7];: 位姿数据，包括位置（3个）和姿态（四元数，4个），总共7个元素。
double mix[18];: 混合参数数组，描述了不同组合状态的数据，例如速度、偏差、里程计、比例因子等。

*/
typedef struct IntegrationState {
    double time;

    Vector3d p{0, 0, 0};
    Quaterniond q{0, 0, 0, 0};
    Vector3d v{0, 0, 0};

    Vector3d bg{0, 0, 0};
    Vector3d ba{0, 0, 0};

    Vector3d s{0, 0, 0};
    double sodo{0};
    Vector2d abv{0, 0};

    Vector3d sg{0, 0, 0};
    Vector3d sa{0, 0, 0};
} IntegrationState;

typedef struct IntegrationStateData {
    double time;

    double pose[7]; // pose : 3 + 4 = 7

    // mix parameters
    // vel + bias : 3 + 6 = 9
    // vel + bias + sodo : 3 + 6 + 1 = 10
    // vel + bias + sodo + abv : 3 + 6 + 1 + 2 = 12
    // vel + bias + scale : 3 + 6 + 6 = 15
    // vel + bias + sodo + scale : 3 + 6 + 1 + 6 = 16
    // vel + bias + sodo + scale + abv : 3 + 6 + 1 + 6 + 2 = 18
    double mix[18];
} IntegrationStateData;

//预积分相关变量
typedef struct IntegrationParameters {
    // IMU噪声为白噪声, 积分为随机游走, 即VRW和ARW
    // 零偏和比例因子建模为一阶高斯马尔卡夫过程, 参数为标准差及相关时间

    double acc_vrw;       // 速度随机游走, m / s^1.5
    double gyr_arw;       // 角度随机游走, rad / s^0.5
    double gyr_bias_std;  // 陀螺零偏标准差, rad / s
    double acc_bias_std;  // 加表零偏标准差, m / s^2
    double gyr_scale_std; // 陀螺比例因子标准差
    double acc_scale_std; // 加表比例因子标准差
    double corr_time;     // 相关时间, s

    double gravity; // 当地重力, m / s^2

    Vector3d odo_std; // 里程计白噪声, m/s
    double odo_srw;   // 里程计比例因子随机游走, PPM / sqrt(Hz)

    Vector3d abv;  // b系与v系的安装角, rad
    Vector3d lodo; // b系下的里程计杆臂, m

    Vector3d station; // 站心坐标系原点
} IntegrationParameters;

#endif // INTEGRATION_DEFINE_H
